/*
 * controller.h
 *
 * Defines all of the structs needed by controller plugins.
 *
 *  Created on: May 3, 2012
 *      Author: Michael Anderson
 */

#ifndef CONTROLLER_H_
#define CONTROLLER_H_

#include <cstdlib>
#include <stdint.h>
#include <vector>
#include <atrias_msgs/controller_status.h>
#include <atrias_msgs/controller_input.h>

using namespace atrias_msgs;

/*
 * This struct is meant to simplify passing around the byte arrays used for
 * communication between the GUI and the controllers.
 *
 * It should be used to return controller input from the guiUpdate function
 * within your controller library.
 *
 * Sometime in the future this struct should be upgraded to use smart pointers.
 *
 * Note: You should normally use the functions in this file to create and
 *       manipulate ByteArrays, but if you decide to allocate the array
 *       yourself be aware that you must use malloc().
 */
typedef struct {
    uint8_t* arr;
    uint16_t len;
} ByteArray;

/*
 * Use this macro to create a new ByteArray of a given size.
 *
 * Do not use this in any main loops! Only call this when a controller is being
 * loaded or unloaded.
 */
#define NEW_BYTE_ARRAY(x) { (uint8_t*)malloc(x), x }

/*
 * Use this macro to cast a struct to a ByteArray
 *
 * Important note: This does not allocate dynamic memory for the array. If you
 *                 need to return a ByteArray created from a struct, use the
 *                 structToByteArray function instead
 */
#define STRUCT_TO_BYTE_ARRAY(x) { (uint8_t*)(x), sizeof(x) }

/*
 * This macro populates a ByteArray with the contents of a byte vector
 */
#define VECTOR_TO_BYTE_ARRAY(vec, bArray) memcpy(bArray.arr, vec.data(), vec.size())

/*
 * Use this macro to cast a ByteArray to a struct when the ByteArray already
 * contains struct data (such as controller status data passed to a guiUpdate
 * function).
 */
#define BYTE_ARRAY_TO_STRUCT(x, type) ((type)(x).arr)

/*
 * This macro frees the memory used internally by a ByteArray. Always use this
 * on a ByteArray once you are finished with it.
 */
#define FREE_BYTE_ARRAY(x) delete (x).arr

/*
 * This struct is used to store the torques generated by your controller. Your
 * controllerUpdate() function will create and return one of these.
 */
typedef struct {
    float motor_torqueA;
    float motor_torqueB;
    float motor_torque_hip;
    bool in_flight;
} ControllerOutput;

/*
 * This struct is useful as a leaner way of passing around robot position and
 * velocity data.
 */
typedef struct {
    float leg_ang;
    float leg_ang_vel;
    float leg_len;
    float leg_len_vel;
    float hip_ang;
    float hip_ang_vel;
} RobotPosition;

/*
 * This struct is used for returning the results of controller and GUI
 * initialization.
 */
typedef struct {
    bool error;
    size_t controllerInputSize;
    size_t controllerStatusSize;
} ControllerInitResult;

/*
 * This struct is used to store data for the controller wrapper.
 */
typedef struct {
    controller_input controllerInput;
    controller_status controllerStatus;
} ControllerWrapperData;

/*
 * This function allows easy conversion from uint8_t vectors used in ros
 * messages and byte arrays that can be cast back into the structs inside.
 */
void byteVectorToArray(std::vector<uint8_t> &data, ByteArray &ba) {
    uint16_t i = 0;
    while (i < data.size()) {
        ba.arr[i] = data[i];
        i++;
    }
    ba.len = i;
}

/*
 * This function populates a uint8_t vector with data from a ByteArray struct.
 * If you wish to convert directly from an arbitrary struct to a vector without
 * converting it to a ByteArray first the next function will perform slightly
 * faster.
 */
void byteArrayToVector(ByteArray &bArray, std::vector<uint8_t> &vec) {
    vec.clear();
    for (uint16_t i = 0; i < bArray.len; i++) {
        vec.push_back(bArray.arr[i]);
    }
}

/*
 * Use this function to serialize a struct and populate a uint8_t vector with
 * it. This saves a lot of time when you need to stuff a struct into a ros
 * message for transmission.
 */
template<class T> void structToVector(T* structPtr, std::vector<uint8_t> *vec) {
    vec->clear();
    uint8_t* bytePtr = (uint8_t*) structPtr;
    for (uint16_t i = 0; i < sizeof(T); i++) {
        vec->push_back(bytePtr[i]);
    }
}

/*
 * Use this function to populate a ByteArray from a struct if you plan on
 * returning the ByteArray.
 */
template<class T> void structToByteArray(T &structRef, ByteArray& ba) {
    memcpy(ba.arr, &structRef, sizeof(T));
    ba.len = sizeof(T);
}

/*
 * Use this function to turn a byte vector from a ros message into a struct.
 *
 * Using this function instead of converting to a ByteArray avoids the hassle
 * of dynamic memory allocation.
 */
template<class T> T vectorToStruct(std::vector<uint8_t> &vec) {
    return *((T*) vec.data());
}

// Control switcher state machine (CSSM) states.
#define CSSM_STATE_DISABLED		0
#define CSSM_STATE_ERROR		1
#define CSSM_STATE_ENABLED		2
#define CSSM_STATE_INIT			3
#define CSSM_STATE_FINI			4
#define CSSM_STATE_LOAD_FAIL	5

#endif /* CONTROLLER_PLUGIN_H_ */
